/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.neu.nutrons.bots.logomotion;

import edu.neu.nutrons.lib.Helpers;

/**
 *
 * @author NUTRONS_PROGRAMMING
 */
public class ElevatorCommand {
    String name;
    double pos; //setpoint
    double lastFB = 0;
    boolean wrist;
    boolean claw;
    double kP;
    double kI;
    double kD;
    final double DEADBAND_FRACTION = 150;

    public ElevatorCommand(String name, double pos, boolean wrist, boolean claw) {
        this.name = name;
        this.pos = pos;
        this.wrist = wrist;
        this.claw = claw;
        // To do: change these!
        kP = 5;
        kI = 0;
        kD = 0;
    }

    public boolean isDone() {
        // Have we gotten there?
        double err = pos - lastFB;
        boolean ret = (err >= -(1./DEADBAND_FRACTION)) && (err <= (1./DEADBAND_FRACTION));
        return ret ;
    }

    public double calcSpeed(double realPos) {
        lastFB = realPos;
        
        double err = pos - realPos;
        double ret = kP * err;
        return Helpers.limitAbs(ret); // Not good
    }
    
    public boolean getWrist() {
        return wrist;
    }
    public boolean getClaw() {
        return claw;
    }

    public double getPos(){
        return pos;
    }

    public void doOffset() {
        ; // Override me!
    }
}
